#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import DrEmpower as dr        # 实物机械臂SDK
import actionlib
import threading
import math
import time

from control_msgs.msg import (
    FollowJointTrajectoryAction,
    FollowJointTrajectoryFeedback,
    FollowJointTrajectoryResult
)
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectoryPoint

# ---------------- 配置区（按需调整） ----------------
# 机械臂关节配置
arm_joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]  # ROS关节名
arm_joint_ids   = [1, 2, 3, 4, 5, 6]  # 硬件ID，对应 DrEmpower

# 夹爪关节配置
gripper_joint_names = ["host_joint"]  # 夹爪关节名
gripper_joint_ids   = [7]  # 夹爪硬件ID

# 合并所有关节
joint_names = arm_joint_names + gripper_joint_names
joint_ids = arm_joint_ids + gripper_joint_ids

joint_state_rate_hz = 50      # /joint_states 发布频率
arm_base_speed = 0.5          # 机械臂最小转速 (r/min)，设小一点让计算速度生效
gripper_base_speed = 0.2      # 夹爪最小转速 (r/min)
gripper_max_speed = 1.5       # 夹爪最大转速（新增：限制夹爪移动速度，避免过冲）
speed_scale = 1.0             # 全局速度放大因子，>1 可以整体加速
ramp_frac = 0.5               # 每段速度最大允许变化比例（50%），用于平滑
arm_accel_param = 20          # 机械臂下发时的 param 参数（平滑加减速，可试调）
gripper_accel_param = 30      # 夹爪下发时的 param 参数
pos_tolerance_deg = 1.0       # 认为到位的角度容差（deg）
gripper_pos_tolerance_deg = 0.5  # 夹爪角度容差（更高精度）
gripper_wait_timeout = 5.0    # 夹爪到位等待超时时间（秒）
use_velocity_field = True     # 优先使用 trajectory 给出的 velocities
dr_retry_limit = 3            # 读写失败重试次数
initial_wait_seconds = 0.6    # execute_cb 开始前等待 joint_states 时间

# Action Server 名称配置
arm_action_server_name = "robot_arm_controller/follow_joint_trajectory"
gripper_action_server_name = "jiazhao_controller/follow_joint_trajectory"
# ---------------------------------------------------


class RealArmBridge:
    def __init__(self):
        rospy.init_node("real_arm_bridge_node")

        # ---------------- 新增：状态同步发布器（向MoveIt推送硬件实时状态） ----------------
        # MoveIt通过 /joint_states 感知机器人状态，确保该话题无延迟、无丢包
        self.js_sync_pub = rospy.Publisher("/joint_states", JointState, queue_size=10)
        # 启动高频状态同步线程（比原50Hz更高，如100Hz，减少延迟）
        self.sync_thread = threading.Thread(target=self.high_freq_state_sync, daemon=True)
        self.sync_thread.start()
        # -----------------------------------------------------------------------------------

        # 串口互斥锁
        self.lock = threading.Lock()

        # joint_states 缓存
        self.latest_positions = [0.0] * len(joint_ids)  # rad（索引对应 joint_ids 顺序）
        self.have_jointstate = threading.Event()

        # 上一次速度缓存（平滑用）
        self.prev_speeds = [arm_base_speed] * len(joint_ids)
        # 为夹爪设置不同的基础速度
        for i in range(len(arm_joint_ids), len(joint_ids)):
            self.prev_speeds[i] = gripper_base_speed

        # 发布器
        self.js_pub = rospy.Publisher("/joint_states", JointState, queue_size=10)

        # 创建两个 Action Server
        self.arm_server = actionlib.SimpleActionServer(
            arm_action_server_name,
            FollowJointTrajectoryAction,
            execute_cb=self.execute_arm_cb,
            auto_start=False
        )
        
        self.gripper_server = actionlib.SimpleActionServer(
            gripper_action_server_name,
            FollowJointTrajectoryAction,
            execute_cb=self.execute_gripper_cb,
            auto_start=False
        )
        
        self.arm_server.start()
        self.gripper_server.start()
        
        rospy.loginfo(f"✅ 机械臂 ActionServer 已启动: {arm_action_server_name}")
        rospy.loginfo(f"✅ 夹爪 ActionServer 已启动: {gripper_action_server_name}")

        # 启动 joint_state 线程（持续读取硬件状态）
        t = threading.Thread(target=self.joint_state_loop, daemon=True)
        t.start()

        # 初始 joint_states 发布
        rospy.loginfo("🔹 等待第一次 joint_states 读取...")
        if not self.have_jointstate.wait(timeout=2.0):
            rospy.logwarn("⚠️ 未能在 2s 内读取到初始 joint_states，MoveIt 可能报错")
        else:
            self.publish_joint_states_once()
            rospy.loginfo("🔹 初始 joint_states 已发布")

    # ---------------- 新增：高频状态同步线程（100Hz推送硬件实时状态） ----------------
    def high_freq_state_sync(self):
        rate = rospy.Rate(100)  # 100Hz，比原50Hz更及时
        while not rospy.is_shutdown():
            try:
                # 直接读取硬件实时状态（不依赖缓存，避免线程延迟）
                real_positions = []
                for jid in joint_ids:
                    ang_deg = self.safe_dr_get_angle(jid)  # 实时读硬件
                    real_positions.append(ang_deg * math.pi / 180.0)  # 转rad
                # 立即发布，强制同步给MoveIt
                js = JointState()
                js.header.stamp = rospy.Time.now()  # 精确时间戳，避免过时
                js.name = joint_names
                js.position = real_positions
                self.js_sync_pub.publish(js)
                # 更新缓存（供其他逻辑使用）
                self.latest_positions = real_positions
                self.have_jointstate.set()
            except Exception as e:
                rospy.logwarn(f"高频状态同步失败: {e}")
            rate.sleep()

    # ---------- 发布一次 joint_states ----------
    def publish_joint_states_once(self):
        js = JointState()
        js.header.stamp = rospy.Time.now()
        js.name = joint_names
        js.position = list(self.latest_positions)
        self.js_pub.publish(js)

    # ---------- 安全单关节下发（核心硬件通信函数，保留原逻辑） ----------
    def safe_dr_set_angle(self, id_num, angle, speed, param, mode=1):
        """
        封装 dr.set_angle，带锁和重试
        angle: deg（硬件SDK要求角度单位）
        speed: SDK 的 speed 参数（r/min）
        """
        for attempt in range(dr_retry_limit):
            try:
                with self.lock:
                    dr.set_angle(id_num=id_num, angle=angle, speed=speed, param=param, mode=mode)
                rospy.logdebug(f"✅ 第{attempt+1}次成功下发：关节{id_num}，目标角度{angle:.2f}°，速度{speed:.2f}r/min")
                return
            except Exception as e:
                rospy.logwarn(f"dr.set_angle({id_num}) 第 {attempt+1}/{dr_retry_limit} 次失败: {e}")
                time.sleep(0.05)
        raise RuntimeError(f"dr.set_angle({id_num}) 连续 {dr_retry_limit} 次失败")

    # ---------- 安全读取角度（核心硬件通信函数，保留原逻辑） ----------
    def safe_dr_get_angle(self, jid):
        for attempt in range(dr_retry_limit):
            try:
                with self.lock:
                    ang = dr.get_angle(id_num=jid)
                return ang  # 返回 deg
            except Exception as e:
                rospy.logwarn(f"dr.get_angle({jid}) 第 {attempt+1}/{dr_retry_limit} 次失败: {e}")
                time.sleep(0.05)
        raise RuntimeError(f"dr.get_angle({jid}) 连续 {dr_retry_limit} 次失败")

    # ---------- joint_states 循环（持续更新硬件状态，保留原逻辑） ----------
    def joint_state_loop(self):
        rate = rospy.Rate(joint_state_rate_hz)
        while not rospy.is_shutdown():
            positions = []
            try:
                for jid in joint_ids:
                    ang_deg = self.safe_dr_get_angle(jid)
                    positions.append(ang_deg * math.pi / 180.0)  # 转 rad 存入缓存
                self.latest_positions = positions
                # 发布关节状态
                js = JointState()
                js.header.stamp = rospy.Time.now()
                js.name = joint_names
                js.position = list(positions)
                self.js_pub.publish(js)
                self.have_jointstate.set()
            except Exception as e:
                rospy.logerr(f"读取关节失败（joint_state_loop）：{e}")
                time.sleep(0.2)
            rate.sleep()

    # ---------- 辅助函数：打印夹爪速度（简化版） ----------
    def print_gripper_speed(self, speed_rpm):
        rospy.loginfo(f"🟢 夹爪目标速度: {speed_rpm:.2f} r/min")

    # ---------- 获取关节索引映射（确保只处理夹爪关节） ----------
    def get_gripper_joint_mapping(self, goal_joint_names):
        """获取规划轨迹中的关节名到本地夹爪关节的映射（只保留匹配的夹爪关节）"""
        mapping = {}
        for goal_idx, name in enumerate(goal_joint_names):
            if name in gripper_joint_names:
                # 找到本地夹爪关节的索引（对应 joint_ids 中的位置）
                local_idx = joint_names.index(name)
                mapping[goal_idx] = local_idx
                rospy.loginfo(f"🔗 匹配夹爪关节: {name}（规划索引{goal_idx} → 本地索引{local_idx}）")
            else:
                rospy.logwarn(f"⚠️ 忽略非夹爪关节: {name}")
        return mapping

    # ------------------- 执行机械臂轨迹（保留原逻辑，未修改） -------------------
    def execute_arm_cb(self, goal):
        rospy.loginfo("📥 收到机械臂轨迹，共 %d 个点", len(goal.trajectory.points))
        result = FollowJointTrajectoryResult()
        result.error_code = FollowJointTrajectoryResult.SUCCESSFUL

        feedback = FollowJointTrajectoryFeedback()
        feedback.joint_names = goal.trajectory.joint_names

        # 获取关节索引映射 - 只映射到机械臂关节
        joint_mapping = self.get_joint_index_mapping(goal.trajectory.joint_names, arm_joint_names)
        if not joint_mapping:
            rospy.logerr("❌ 没有找到匹配的机械臂关节名，无法执行轨迹")
            result.error_code = FollowJointTrajectoryResult.INVALID_JOINTS
            self.arm_server.set_aborted(result)
            return

        rospy.sleep(initial_wait_seconds)
        if not self.have_jointstate.is_set():
            rospy.logwarn("⚠️ 执行时尚未收到 joint_states，可能 MoveIt 认为无效")

        points = goal.trajectory.points
        if len(points) < 2:
            rospy.logwarn("⚠️ 轨迹点不足，直接完成")
            self.arm_server.set_succeeded(result)
            return

        for idx in range(1, len(points)):
            if self.arm_server.is_preempt_requested():
                rospy.loginfo("⏹️ 收到 preempt 请求，停止执行机械臂轨迹")
                self.arm_server.set_preempted()
                return

            prev_point = points[idx - 1]
            curr_point = points[idx]

            dt = (curr_point.time_from_start - prev_point.time_from_start).to_sec()
            if dt <= 0:
                dt = 0.01

            target_deg = [0] * len(arm_joint_names)
            speed_list = [0] * len(arm_joint_names)

            # 计算并下发每个关节的速度（r/min）
            for goal_idx, arm_idx in joint_mapping.items():
                jid = arm_joint_ids[arm_idx]
                p_prev = prev_point.positions[goal_idx]
                p_curr = curr_point.positions[goal_idx]

                # 优先使用 velocity 字段
                vel_rad_s = None
                if use_velocity_field and hasattr(curr_point, "velocities") and len(curr_point.velocities) > goal_idx:
                    vel = curr_point.velocities[goal_idx]
                    if vel is not None and abs(vel) > 1e-6:
                        vel_rad_s = vel
                if vel_rad_s is None:
                    vel_rad_s = (p_curr - p_prev) / dt

                vel_deg_s = abs(vel_rad_s) * 180.0 / math.pi
                rpm = (vel_deg_s / 6.0) * speed_scale

                # 平滑 ramp
                prev_sp = self.prev_speeds[arm_idx] if self.prev_speeds else arm_base_speed
                if prev_sp <= 0:
                    prev_sp = arm_base_speed
                min_allowed = max(prev_sp * (1 - ramp_frac), arm_base_speed)
                max_allowed = prev_sp * (1 + ramp_frac)
                rpm_smoothed = min(max(rpm, min_allowed), max_allowed)

                cmd_speed = max(arm_base_speed, rpm_smoothed)

                # 下发到机械臂
                try:
                    self.safe_dr_set_angle(id_num=jid, angle=p_curr * 180.0 / math.pi,
                                        speed=cmd_speed, param=arm_accel_param, mode=1)
                except Exception as e:
                    rospy.logerr(f"❌ 机械臂 dr.set_angle 失败: {e}")
                    result.error_code = FollowJointTrajectoryResult.INVALID_GOAL
                    self.arm_server.set_aborted(result)
                    return

                self.prev_speeds[arm_idx] = cmd_speed
                target_deg[arm_idx] = round(p_curr * 180.0 / math.pi, 2)
                speed_list[arm_idx] = round(cmd_speed, 2)

            # 输出速度
            self.print_joint_speeds(speed_list, idx=idx, controller_type="arm")

            rospy.loginfo(f"⬆️ 机械臂段 {idx}/{len(points)-1}: targets={target_deg}, dt={dt:.3f}s, speeds(r/min)={speed_list}")

            # 等待该段完成
            t_deadline = rospy.Time.now() + rospy.Duration.from_sec(dt)
            while rospy.Time.now() < t_deadline and not rospy.is_shutdown():
                # 发布反馈
                js = JointState()
                js.header.stamp = rospy.Time.now()
                js.name = joint_names
                js.position = list(self.latest_positions)
                self.js_pub.publish(js)

                feedback.header = js.header
                feedback.actual.positions = js.position
                
                # 构建期望位置数组（按机械臂关节顺序）
                desired_positions = [0.0] * len(joint_names)
                for goal_idx, arm_idx in joint_mapping.items():
                    desired_positions[arm_idx] = curr_point.positions[goal_idx]
                
                feedback.desired.positions = desired_positions
                feedback.error.positions = [a - b for a, b in zip(js.position, desired_positions)]
                self.arm_server.publish_feedback(feedback)

                # 检查是否到位
                all_reached = True
                for goal_idx, arm_idx in joint_mapping.items():
                    p_curr = curr_point.positions[goal_idx]
                    actual_deg = self.latest_positions[arm_idx] * 180.0 / math.pi
                    target_deg_val = p_curr * 180.0 / math.pi
                    
                    if abs(actual_deg - target_deg_val) > pos_tolerance_deg:
                        all_reached = False
                        break
                
                if all_reached:
                    break

                rospy.sleep(0.02)  # 50Hz 更新

        rospy.loginfo("✅ 机械臂轨迹执行完成")
        self.arm_server.set_succeeded(result)

    # 修正：硬件状态同步到MoveIt（解决try块报错问题）
    def sync_hardware_state_to_moveit(self):
        """主动发布硬件实时状态，让MoveIt更新规划起始状态"""
        try:
            # 读取所有关节实时状态（增加超时判断，避免卡死）
            real_positions = []
            start_read_time = rospy.Time.now()
            for jid in joint_ids:
                # 单个关节读取超时保护（0.5秒）
                if (rospy.Time.now() - start_read_time).to_sec() > 0.5:
                    raise TimeoutError(f"读取关节{jid}超时，可能硬件连接异常")
                ang_deg = self.safe_dr_get_angle(jid)
                real_positions.append(ang_deg * math.pi / 180.0)  # 转弧度
            
            # 连续发布5次（比3次更可靠），间隔10ms确保MoveIt收到
            js = JointState()
            js.name = joint_names
            js.position = real_positions
            for i in range(5):
                js.header.stamp = rospy.Time.now()  # 每次发布更新时间戳
                self.js_sync_pub.publish(js)
                rospy.sleep(0.01)
            
            rospy.loginfo("🔄 已将硬件实时状态同步给MoveIt，规划起始状态校准完成")
            return True  # 同步成功
        
        except TimeoutError as te:
            rospy.logerr(f"❌ 状态同步超时: {te}")
        except Exception as e:
            # 具体说明错误类型，方便调试
            rospy.logerr(f"❌ 状态同步失败（{type(e).__name__}）: {str(e)}")
        
        return False  # 同步失败

    # ------------------- 【重点修改】执行夹爪轨迹（仅取最后一个目标点） -------------------
    def execute_gripper_cb(self, goal):
        # 新增：规划前强制同步硬件状态给MoveIt（消除初始偏差）
        self.sync_hardware_state_to_moveit()
        rospy.loginfo("📥 收到夹爪轨迹，共 %d 个点（仅提取最后一个作为目标）", len(goal.trajectory.points))
        result = FollowJointTrajectoryResult()
        result.error_code = FollowJointTrajectoryResult.SUCCESSFUL

        # 1. 检查轨迹点有效性
        points = goal.trajectory.points
        if len(points) == 0:
            rospy.logerr("❌ 轨迹无有效点，无法执行")
            result.error_code = FollowJointTrajectoryResult.INVALID_GOAL
            self.gripper_server.set_aborted(result)
            return
        # 提取最后一个点作为目标（忽略所有中间点）
        target_point = points[-1]
        rospy.loginfo(f"🎯 提取夹爪最终目标点：时间戳{target_point.time_from_start.to_sec():.2f}s")

        # 2. 匹配夹爪关节（过滤非夹爪关节）
        joint_mapping = self.get_gripper_joint_mapping(goal.trajectory.joint_names)
        if not joint_mapping:
            rospy.logerr("❌ 没有找到匹配的夹爪关节名（如host_joint），无法执行")
            result.error_code = FollowJointTrajectoryResult.INVALID_JOINTS
            self.gripper_server.set_aborted(result)
            return

        # 3. 等待关节状态就绪
        rospy.sleep(initial_wait_seconds)
        if not self.have_jointstate.is_set():
            rospy.logwarn("⚠️ 执行前未收到关节状态，可能影响到位判断")

        # 4. 计算夹爪目标角度和速度
        for goal_idx, local_idx in joint_mapping.items():
            # local_idx：夹爪关节在 joint_ids 中的索引（对应硬件ID）
            gripper_jid = joint_ids[local_idx]  # 夹爪硬件ID（如7）
            # 目标位置转换：ROS轨迹是弧度，转角度（硬件SDK要求）
            target_pos_rad = target_point.positions[goal_idx]
            target_pos_deg = target_pos_rad * 180.0 / math.pi
            # 当前位置（从缓存读取，已转换为rad）
            current_pos_rad = self.latest_positions[local_idx]
            current_pos_deg = current_pos_rad * 180.0 / math.pi

            rospy.loginfo(f"📊 夹爪当前状态：关节{gripper_jid}，当前角度{current_pos_deg:.2f}°，目标角度{target_pos_deg:.2f}°")

            # 计算目标速度（基于规划器速度字段或位置差，限制最大速度避免过冲）
            # 计算目标速度（优化后：基于距离动态调整速度）
            cmd_speed = gripper_base_speed
            pos_diff_deg = abs(target_pos_deg - current_pos_deg)  # 当前与目标的距离

            if pos_diff_deg > 10:  # 距离远 → 用最大速度，提高效率
                cmd_speed = gripper_max_speed
            elif pos_diff_deg > 3:  # 距离中等 → 用中速
                cmd_speed = gripper_max_speed * 0.7
            else:  # 距离近（<3°）→ 用低速，避免过冲
                cmd_speed = gripper_max_speed * 0.3

            # 优先使用规划器速度（但限制在动态计算的速度范围内）
            if use_velocity_field and hasattr(target_point, "velocities") and len(target_point.velocities) > goal_idx:
                vel_rad_s = target_point.velocities[goal_idx]
                if abs(vel_rad_s) > 1e-6:
                    vel_deg_s = abs(vel_rad_s) * 180.0 / math.pi
                    planner_speed = (vel_deg_s / 6.0) * speed_scale
                    # 限制规划器速度在动态速度范围内
                    cmd_speed = max(gripper_base_speed, min(planner_speed, cmd_speed))

            # 5. 下发目标位置到夹爪硬件
            try:
                self.safe_dr_set_angle(
                    id_num=gripper_jid,
                    angle=target_pos_deg,
                    speed=cmd_speed,
                    param=gripper_accel_param,
                    mode=1  # 按角度模式下发
                )
            except Exception as e:
                rospy.logerr(f"❌ 夹爪下发失败：{e}")
                result.error_code = FollowJointTrajectoryResult.INVALID_GOAL
                self.gripper_server.set_aborted(result)
                return

            # 6. 等待夹爪到位（优化后：实时读硬件+动态容差+二次修正）
            rospy.loginfo(f"⌛ 等待夹爪关节{gripper_jid}到位（基础容差{gripper_pos_tolerance_deg}°，超时{gripper_wait_timeout}s）")
            start_time = rospy.Time.now()
            retry_correction = 0  # 二次修正次数（最多2次）
            max_correction = 2     # 最大修正次数，避免死循环
            final_tolerance = gripper_pos_tolerance_deg * 1.2  # 最终允许的“软容差”（比基础容差宽20%）

            while not rospy.is_shutdown():
                # 关键：直接读取硬件实时状态，不依赖缓存（避免延迟）
                current_actual_deg = self.safe_dr_get_angle(gripper_jid)
                pos_error_deg = abs(current_actual_deg - target_pos_deg)
                rospy.logdebug(f"🔍 实时状态：当前{current_actual_deg:.2f}°，目标{target_pos_deg:.2f}°，误差{pos_error_deg:.2f}°")

                # 条件1：误差小于基础容差 → 直接到位
                if pos_error_deg <= gripper_pos_tolerance_deg:
                    rospy.loginfo(f"✅ 夹爪到位（误差{pos_error_deg:.2f}° ≤ 基础容差{gripper_pos_tolerance_deg}°）")
                    break

                # 条件2：误差小于软容差，且持续100ms → 判定到位（避免微小抖动导致误判）
                if pos_error_deg <= final_tolerance:
                    # 检测误差是否稳定在软容差内
                    stable_start = rospy.Time.now()
                    while (rospy.Time.now() - stable_start).to_sec() < 0.1:  # 稳定100ms
                        if abs(self.safe_dr_get_angle(gripper_jid) - target_pos_deg) > final_tolerance:
                            break
                    else:  # 100ms内误差未超出 → 判定到位
                        rospy.loginfo(f"✅ 夹爪稳定到位（误差{pos_error_deg:.2f}° ≤ 软容差{final_tolerance}°，稳定100ms）")
                        break

                # 条件3：超时 → 尝试二次修正（若未达最大修正次数）
                if (rospy.Time.now() - start_time).to_sec() > gripper_wait_timeout:
                    if retry_correction < max_correction:
                        # 二次修正：基于当前实际位置，重新计算目标（微调，避免大幅偏差）
                        correction = (target_pos_deg - current_actual_deg) * 0.8  # 按80%误差修正
                        new_target_deg = current_actual_deg + correction
                        rospy.loginfo(f"🔄 超时未到位，尝试第{retry_correction+1}次修正：新目标{new_target_deg:.2f}°（原目标{target_pos_deg:.2f}°）")
                        # 重新下发修正后的目标
                        self.safe_dr_set_angle(
                            id_num=gripper_jid,
                            angle=new_target_deg,
                            speed=cmd_speed * 0.5,  # 降速修正，提高精度
                            param=gripper_accel_param,
                            mode=1
                        )
                        retry_correction += 1
                        start_time = rospy.Time.now()  # 重置超时计时
                    else:
                        # 修正次数用尽，仍未到位 → 判定“精度不满足但允许运动完成”（避免卡死）
                        rospy.logwarn(f"⚠️ 已达最大修正次数（{max_correction}次），误差{pos_error_deg:.2f}°（≤软容差{final_tolerance}°），强制判定到位")
                        break

                rospy.sleep(0.01)  # 100Hz高频检查，减少延迟

        # 7. 执行完成，反馈成功
        rospy.loginfo("🎉 夹爪轨迹执行完成（仅执行最终目标点）")
        self.gripper_server.set_succeeded(result)


    # ------------------- 保留原辅助函数 -------------------
    def print_joint_speeds(self, speeds_rpm, idx=None, controller_type="arm"):
        if idx is not None:
            rospy.loginfo(f"🟢 {controller_type} 段 {idx}: 关节速度 (r/min) = {speeds_rpm}")
        else:
            rospy.loginfo(f"🟢 {controller_type} 当前关节速度 (r/min) = {speeds_rpm}")

    def get_joint_index_mapping(self, goal_joint_names, target_joint_names):
        mapping = {}
        for i, name in enumerate(goal_joint_names):
            if name in target_joint_names:
                mapping[i] = target_joint_names.index(name)
            else:
                rospy.logwarn(f"未知关节名: {name}")
        return mapping


if __name__ == "__main__":
    try:
        RealArmBridge()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass